#include "apriltag_opencv.h"
#include "apriltag_family.h"
#include "pose.h"
#include "zarray.h"
#include "homography.h"
#include "getopt.h"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include "pose_estimate.h"
// #include "serial.h"
#include "controllor.h"
#include "connector.h"
#include <string>

#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include "json.hpp"
using json = nlohmann::json;

bool USE_GUI = false;
bool USE_VIDEO = true;

#define USE_CONTOUR false
double fx = 616.5409/2;
double fy = 615.8232/2;
double cx = 160;
double cy = 120;
// const double fx = 764, fy = 764, cx = 408, cy = 306;
double tagsize = 2.9;//4.6

int target_id = 8;
bool is_stop = false;
int camera_index = 0;

std::string movie_file;// = "../images/test/test.mp4";

void update_param(void)
{
    std::ifstream file("../parameters.json");
    json j;
    file >> j; 
    fx = j["camera_info"]["fx"];
    fy = j["camera_info"]["fy"];
    cx = j["camera_info"]["cx"];
    cy = j["camera_info"]["cy"];
    cy = j["camera_info"]["cy"];
    tagsize = j["apriltag_info"]["size"];
    target_id = j["apriltag_info"]["target_id"];
    camera_index = j["camera_info"]["camera_index"];
    USE_VIDEO = j["use_video"];
    USE_GUI = j["use_gui"];
    movie_file = j["video_file"];
    // std::cout << j << std::endl;

}

void end_handler(int s){
    printf("Caught signal %d\n",s);
    is_stop = true;
    // exit(1); 
}


void print_mat(const double* src, int nrows, int ncols, const char* fmt) {
    int cnt = 0;
    for (int i=0; i<nrows; ++i) {
        for (int j=0; j<ncols; ++j) {
            printf(fmt, src[cnt++]);
        }
        printf("\n");
    }
}
void print_vec3(const double* v) {
    print_mat(v, 1, 3, "%12f, ");
}

void draw_result(cv::Mat& img, const double corners[][2], const int id, const int margin)
{
    std::vector<cv::Point> pts ;
    for(size_t i = 0; i < 4; i++)
    {
        pts.push_back(cv::Point(corners[i][0],corners[i][1]));
    }    
    polylines( img, pts, 1, cv::Scalar(0,0,256), 3, 8, 0);
    putText(img, std::to_string(id)+" : "+std::to_string(margin), pts.at(1), cv::FONT_HERSHEY_SIMPLEX, 0.8, cv::Scalar(0, 0, 255),2);
}

int main(int argc, char** argv) {
    // serial_init();
    signal(SIGINT, end_handler);
    update_param();

    const char *famname = "tag16h5"; //getopt_get_string(getopt, "family");
    apriltag_family_t *tf = apriltag_family_create(famname);

    if (!tf) {
        printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
        exit(-1);
    }

    tf->black_border = 1;//getopt_get_int(getopt, "border");

    apriltag_detector_t *detector = apriltag_detector_create();
    apriltag_detector_add_family(detector, tf);

    if (USE_CONTOUR) {
        apriltag_detector_enable_quad_contours(detector, 1);
    }
        
    detector->quad_decimate = 1.0;
    detector->quad_sigma = 0.0; // blur
    detector->nthreads = 4;
    detector->debug = 0;
    detector->refine_edges = 1;
    detector->refine_decode = 0;
    detector->refine_pose = 0;

    

    
    

    cv::VideoCapture* cap;

    if (USE_VIDEO) {
        std::cout << " open video file"<< movie_file<<  std::endl;
        cap = new cv::VideoCapture(movie_file);
    } 
    else {
        std::cout << " open camera "<< camera_index << std::endl;
        #if CV_MAJOR_VERSION > 3
            cap = new cv::VideoCapture(camera_index, cv::CAP_V4L);
        #else
            cap = new cv::VideoCapture(camera_index);
        #endif
        cap->set(cv::CAP_PROP_FRAME_HEIGHT, 240);
        cap->set(cv::CAP_PROP_FRAME_WIDTH, 320);
        cap->set(cv::CAP_PROP_AUTO_EXPOSURE, 0.75); 
    }

    const char* window = "Camera";

    cv::Mat frame, gray, display;
    // frame = cv::imread("../images/test/2.jpg");
    // std::cout << "size = "<< frame.cols << frame.rows << std::endl;

    controllor ctl;

    std::shared_ptr<connector<float, float, float, int>> con = std::make_shared<connector<float, float, float, int>>();

    ctl.set_connector(con);

    ctl.start();

    int cnt = 0;
    while (!is_stop) {
        cnt++;
        bool ok = cap->read(frame);
        if (!ok) { std::cout << "video fail "<< std::endl; break; }
        display = frame;
        // std::cout << "size = "<< frame.cols << frame.rows << std::endl;
        // cv::imshow(window, frame);
        int64 t0 = cv::getTickCount();
        cv::cvtColor(frame, gray, cv::COLOR_RGB2GRAY);

        
        // Mat8uc1 gray;
        image_u8_t* im8 = cv2im8_copy(gray);
        
        
        zarray_t *detections = apriltag_detector_detect(detector, im8);
        // std::cout << "detections " << std::endl;
        int64 t1 = cv::getTickCount();
        float secs = (t1-t0)*1000.0/cv::getTickFrequency();

        // printf("detected %d tags in %d ms\n", zarray_size(detections), int(secs));

        // cv::Mat display = detectionsImage(detections, frame.size(), frame.type());
        bool updated = false;
        for (int i = 0; i < zarray_size(detections); i++) {
            apriltag_detection_t *det;
            zarray_get(detections, i, &det);
            if(det->decision_margin > 40)
            {
                /* pose estimation*/
                pose_t pose;
                estimate_pose_from_vertex(det->p, &pose, tagsize, fx, fy, cx, cy);
                // std::cout << "estimate_pose_from_vertex " << std::endl;
                if(det->id == target_id){
                    // send_cmd(100,0);
                    // send_cmd(pose.tvec[2],pose.tvec[0]);
                    //  std::cout << "connetor " << std::endl;
                    con->write(pose.tvec[0], pose.tvec[1], pose.tvec[2], cnt);
                    updated = true;
                    // printf("detection %3d: id = %3d hamming %d, " //id (%2dx%2d)-%-4d,
                    // "goodness %8.3f, margin %8.3f\n",
                    // i, //det->family->d*det->family->d, det->family->h,
                    // det->id, det->hamming, det->goodness, det->decision_margin);
                    
                    printf("t-vector = "); print_vec3(pose.tvec);
                }

                // draw_result(display, det->p, det->id, det->decision_margin);
            }
            
        
        }
        // if(!updated)
        //     send_reset();

        printf("\n");

        apriltag_detections_destroy(detections);

        
        image_u8_destroy(im8);
        if(USE_GUI)
        {
            cv::imshow(window, display);
            int k = cv::waitKey(1);
            if (k == 27) { 
                break; 
            }
        }
        
        
    }

    cap->release();
    ctl.~controllor();
    std::cout << "main thread exit" << std::endl;
    return 0;

}